#include "spckAdapter.h"
#include "stringSplit.h"
#include <vector>

 SpckAdapter::SpckAdapter(string filePath)
{
		
	fstream fin;
	fin.open(filePath, ios::in);
	if (!fin.is_open())
	{
		printf("can not get the simpack input file(*.spck).");		
	}
	vector<string> fileLines;
	string fileLine;
	while (getline(fin, fileLine))
	{
		fileLines.push_back(fileLine);
	}
	fin.close();

	for (int i = 0; i < fileLines.size(); i++)
	{
		string varLine = fileLines[i];
		if (varLine[0] != '!'&& varLine.size()>0)
		{			
			spackVariables.push_back(GetVariableValue(varLine));
		}
	}
}

 vector<SpckCosimBody> SpckAdapter::GetSpckCosimBody()
 {
	 vector<SpckCosimBody> spckCosimBodies;		 
	 vector<SpckOutVariable> outVariables = GetOutVariables();
	 vector<SpckInVariable> inVariables = GetInVariables();

	 for (int i = 0; i < outVariables.size(); i++)
	 {
		 SpckCosimBody cosimBody;
		 cosimBody.name = outVariables[i].body;
		 bool isNewBody = true;
		 for (int j = 0; j < spckCosimBodies.size(); j++)
		 {
			 if (outVariables[i].body == spckCosimBodies[j].name)
			 {
				 isNewBody = false;
			 }
		 }
		 if (isNewBody == true)
		 {
			 spckCosimBodies.push_back(cosimBody);
		 }
	 }
	 
	 for (int i = 0; i < spckCosimBodies.size(); i++)
	 {
		 for (int j = 0; j < spackVariables.size(); j++)
		 {
			 SpckVariable variable = spackVariables[j];
			 if (variable.name == "body.m")
			 {
				 if (variable.items[0] == spckCosimBodies[i].name)
				 {
					 spckCosimBodies[i].mass = stod(variable.value);
				 }
			 }
			 if (variable.name == "body.I.tens")
			 {
				 if (variable.items[2] == spckCosimBodies[i].name)
				 {
					 int m = stoi(variable.items[0]) - 1;
					 int n = stoi(variable.items[1]) - 1;
					 spckCosimBodies[i].momentOfInertia[m][n] = stod(variable.value);
				 }
			 }
		 }
		 for (int j = 0; j < outVariables.size(); j++)
		 {
			 if (spckCosimBodies[i].name == outVariables[j].body)
			 {
				 switch (outVariables[j].outPutType)
				 {
				 case POS_X:
					 spckCosimBodies[i].pos_x_name = outVariables[j].name;
					 break;
				 case POS_Y:
					 spckCosimBodies[i].pos_y_name = outVariables[j].name;
					 break;
				 case POS_Z:
					 spckCosimBodies[i].pos_z_name = outVariables[j].name;
					 break;
				 case ORT_X:
					 spckCosimBodies[i].ort_x_name = outVariables[j].name;
					 break;
				 case ORT_Y:
					 spckCosimBodies[i].ort_y_name = outVariables[j].name;
					 break;
				 case ORT_Z:
					 spckCosimBodies[i].ort_z_name = outVariables[j].name;
					 break;
				 case VEL_X:
					 spckCosimBodies[i].vel_x_name = outVariables[j].name;
					 break;
				 case VEL_Y:
					 spckCosimBodies[i].vel_y_name = outVariables[j].name;
					 break;
				 case VEL_Z:
					 spckCosimBodies[i].vel_z_name = outVariables[j].name;
					 break;
				 case ANG_VEL_X:
					 spckCosimBodies[i].ang_vel_x_name = outVariables[j].name;
					 break;
				 case ANG_VEL_Y:
					 spckCosimBodies[i].ang_vel_y_name = outVariables[j].name;
					 break;
				 case ANG_VEL_Z:
					 spckCosimBodies[i].ang_vel_z_name = outVariables[j].name;
					 break;
				 default:
					 spckCosimBodies[i].pos_x_name = outVariables[j].name;
					 break;
				 }
			 }
		 }
		 for (int j = 0; j < inVariables.size(); j++)
		 {
			 if (spckCosimBodies[i].name == inVariables[j].body)
			 {
				 switch (inVariables[j].inPutType)
				 {
				 case F_X:
					 spckCosimBodies[i].fx_name = inVariables[j].name;
					 break;
				 case F_Y:
					 spckCosimBodies[i].fy_name = inVariables[j].name;
					 break;
				 case F_Z:
					 spckCosimBodies[i].fz_name = inVariables[j].name;
					 break;
				 case T_X:
					 spckCosimBodies[i].tx_name = inVariables[j].name;
					 break;
				 case T_Y:
					 spckCosimBodies[i].ty_name = inVariables[j].name;
					 break;
				 case T_Z:
					 spckCosimBodies[i].tz_name = inVariables[j].name;
					 break;				
				 default:
					 break;
				 }
			 }
		 }
	 }	
	 return spckCosimBodies;
 }
 // get all the Variables of simpack file
SpckVariable SpckAdapter::GetVariableValue(string variable)
{
	SpckVariable spckVariable;
	vector<string> tempVariables = StringSplit::SplitWithStl(variable, "=");
	if (tempVariables.size() > 1)
	{
		string temp1 = tempVariables[0];
		spckVariable.name = temp1.substr(0, temp1.find_first_of('('));
		spckVariable.name = StringSplit::Trim(spckVariable.name);
		if (temp1.find_first_of('(') != temp1.npos)
		{
			string temp2 = temp1.substr(temp1.find_first_of('(')+1, temp1.find_first_of(')')- temp1.find_first_of('(')-1);
			if (temp2.size() > 0)
			{
				vector<string> tempItems = StringSplit::SplitWithStl(temp2, ",");
				for (int i = 0; i < tempItems.size(); i++)
				{
					spckVariable.items.push_back(tempItems[i]);
				}
			}
		}

		string temp2 = tempVariables[1];
		if (temp2.find_first_of('!') != temp2.npos)
		{
			spckVariable.value = temp2.substr(0, temp2.find_first_of('!'));
			spckVariable.value = StringSplit::Trim(spckVariable.value);
			spckVariable.comment = temp2.substr(temp2.find_first_of('!') + 1);
			spckVariable.comment = StringSplit::Trim(spckVariable.comment);
		}
		else
		{
			spckVariable.value = temp2;
		}
	}

	return spckVariable;
}

// get all the output Variables from simpack file
vector<SpckOutVariable> SpckAdapter::GetOutVariables()
{
	vector<SpckOutVariable> spckOutVariables;
	for (int i = 0; i < spackVariables.size(); i++)
	{
		SpckVariable variable = spackVariables[i];
		if (variable.name == "yout.type")
		{
			SpckOutVariable spckOutVariable;
			spckOutVariable.name = variable.items[0];	
			spckOutVariable.type = stoi(variable.value);
			spckOutVariables.push_back(spckOutVariable);
		}
	}


	for (int i = 0; i < spckOutVariables.size(); i++)
	{
		string sensor;
		for (int j = 0; j < spackVariables.size(); j++)
		{
			if (spackVariables[j].name == "yout.par")
			{
				if (spackVariables[j].items[0] == "1" && spackVariables[j].items[1] == spckOutVariables[i].name)
				{
					sensor = spackVariables[j].value;
				}
				if (spackVariables[j].items[0] == "2" && spackVariables[j].items[1] == spckOutVariables[i].name)
				{
					if (spckOutVariables[i].type == 2)
					{
						double direction = stod(spackVariables[j].value);
						if (direction == 1)
						{
							spckOutVariables[i].outPutType = POS_X;
						}
						else if (direction == 2)
						{
							spckOutVariables[i].outPutType = POS_Y;
						}
						else if (direction == 3)
						{
							spckOutVariables[i].outPutType = POS_Z;
						}

					}
					else if (spckOutVariables[i].type == 3)
					{
						double direction = stod(spackVariables[j].value);
						if (direction == 1)
						{
							spckOutVariables[i].outPutType = ORT_X;
						}
						else if (direction == 2)
						{
							spckOutVariables[i].outPutType = ORT_Y;
						}
						else if (direction == 3)
						{
							spckOutVariables[i].outPutType = ORT_Z;
						}
					}
					else if (spckOutVariables[i].type == 4)
					{
						double direction = stod(spackVariables[j].value);
						if (direction == 1)
						{
							spckOutVariables[i].outPutType = VEL_X;
						}
						else if (direction == 2)
						{
							spckOutVariables[i].outPutType = VEL_Y;
						}
						else if (direction == 3)
						{
							spckOutVariables[i].outPutType = VEL_Z;
						}
					}
					else if (spckOutVariables[i].type == 5)
					{
						double direction = stod(spackVariables[j].value);
						if (direction == 1)
						{
							spckOutVariables[i].outPutType = ANG_VEL_X;
						}
						else if (direction == 2)
						{
							spckOutVariables[i].outPutType = ANG_VEL_Y;
						}
						else if (direction == 3)
						{
							spckOutVariables[i].outPutType = ANG_VEL_Z;
						}
					}
				}
			}
		}

		string toMaker;
		for (int j = 0; j < spackVariables.size(); j++)
		{
			if (spackVariables[j].name == "sensor.to")
			{
				if (spackVariables[j].items[0] == sensor)
				{
					toMaker = spackVariables[j].value;
				}
			}
		}

		for (int j = 0; j < spackVariables.size(); j++)
		{
			if (spackVariables[j].name == "marker.parent")
			{
				if (spackVariables[j].items[0] == toMaker)
				{
					spckOutVariables[i].body = spackVariables[j].value;
				}
			}
		}


	}

	return spckOutVariables;
}
vector< SpckInVariable> SpckAdapter::GetInVariables()
{
	vector<SpckInVariable> spckInVariables;
	for (int i = 0; i < spackVariables.size(); i++)
	{
		SpckVariable variable = spackVariables[i];
		if (variable.name == "uinp.idx")
		{
			SpckInVariable spckInVariable;
			spckInVariable.name = variable.items[0];			
			spckInVariables.push_back(spckInVariable);
		}
	}


	for (int i = 0; i < spckInVariables.size(); i++)
	{
		string inVariableName = spckInVariables[i].name;
		string uVector;

		//get uVector from Input Variable Name
		for (int j = 0; j < spackVariables.size(); j++)
		{
			if (spackVariables[j].name == "uele.uinp")
			{
				if (spackVariables[j].value == inVariableName)
				{
					uVector = spackVariables[j].items[0];
				}
			}
		}

		//get inPutType  and u-Vector element force from uVector
		string forceElement;
		for (int j = 0; j < spackVariables.size(); j++)
		{
			if (spackVariables[j].name == "force.par")
			{
				if (spackVariables[j].value == uVector)
				{
					forceElement = spackVariables[j].items[1];
					switch (stoi(spackVariables[j].items[0]))
					{
					case 1:
						spckInVariables[i].inPutType = F_X;
						break;
					case 2:
						spckInVariables[i].inPutType = F_Y;
						break;
					case 3:
						spckInVariables[i].inPutType = F_Z;
						break;
					case 4:
						spckInVariables[i].inPutType = T_X;
						break;
					case 5:
						spckInVariables[i].inPutType = T_Y;
						break;
					case 6:
						spckInVariables[i].inPutType = T_Z;
						break;
					default:
						break;
					}
				}
			}			
		}

		//get to maker from u-Vector element force
		string forceToMarker;
		for (int j = 0; j < spackVariables.size(); j++)
		{
			if (spackVariables[j].name == "force.to")
			{
				if (spackVariables[j].items[0] == forceElement)
				{
					forceToMarker = spackVariables[j].value;
				}
			}
		}

		// get body from to maker
		for (int j = 0; j < spackVariables.size(); j++)
		{
			if (spackVariables[j].name == "marker.parent")
			{
				if (spackVariables[j].items[0] == forceToMarker)
				{
					spckInVariables[i].body = spackVariables[j].value;
				}
			}
		}

	}
	
	return spckInVariables;
}

